#include "imu.h"
imu_data mpu6050_zero;
imu_data mpu6050_data;
imu_data mpu6050_data_lpf;
MPU6050 imu;
angle_data X_Roll;
angle_data Y_Pitch;
angle_data Z_Yaw;
void mpu6050_init()
{
    Serial.begin(115200);
    Serial.println("initializing I2C...");
    // //D4->SDA
    // //D5->SCL

    Wire.begin();
    Wire.setClock(400000);

    imu.initialize();

    Serial.println("Testing device!");
    if (imu.testConnection())
    {
        Serial.println("MPU6050 successful");
    }
    else
    {
        while (1)
            Serial.println("MPU6050 failed");
    }
    for (int i = 0; i < 200; i++)
    {
        imu.getMotion6(&mpu6050_data.acc_x, &mpu6050_data.acc_y, &mpu6050_data.acc_z, &mpu6050_data.gyro_x, &mpu6050_data.gyro_y, &mpu6050_data.gyro_z);
        mpu6050_zero.gyro_x += mpu6050_data.gyro_x;
        mpu6050_zero.gyro_y += mpu6050_data.gyro_y;
        mpu6050_zero.gyro_z += mpu6050_data.gyro_z;
    }

    mpu6050_zero.gyro_x = mpu6050_zero.gyro_x / 200;
    mpu6050_zero.gyro_y = mpu6050_zero.gyro_y / 200;
    mpu6050_zero.gyro_z = mpu6050_zero.gyro_z / 200;
}
void Get_Attitude()
{
    X_Roll.AngleAtan = (atan2(mpu6050_data.acc_y, mpu6050_data.acc_z)) * 180 / PI;
    X_Roll_Angle_ComplementaryFilter(X_Roll.AngleAtan, (mpu6050_data_lpf.gyro_x));

    Y_Pitch.AngleAtan = (atan2(mpu6050_data.acc_x, mpu6050_data.acc_z)) * 180 / PI;
    Y_Pitch_Angle_ComplementaryFilter(Y_Pitch.AngleAtan, mpu6050_data_lpf.gyro_y);

    Z_Yaw.AngleAtan = (atan2(mpu6050_data.acc_y, mpu6050_data.acc_x)) * 180 / PI;
    Z_Yaw_Angle_ComplementaryFilter(Z_Yaw.AngleAtan, mpu6050_data_lpf.gyro_z);
}
void mpu6050_read()
{
    imu.getMotion6(&mpu6050_data.acc_x, &mpu6050_data.acc_y, &mpu6050_data.acc_z, &mpu6050_data.gyro_x, &mpu6050_data.gyro_y, &mpu6050_data.gyro_z);

    mpu6050_data.gyro_x = mpu6050_data.gyro_x - mpu6050_zero.gyro_x;
    mpu6050_data.gyro_y = mpu6050_data.gyro_y - mpu6050_zero.gyro_y;
    mpu6050_data.gyro_z = mpu6050_data.gyro_z - mpu6050_zero.gyro_z;
    imu_filter();

    Get_Attitude();

    X_Roll.FinalAngle = X_Roll.FuseAngle + 20;
    Y_Pitch.FinalAngle = Y_Pitch.FuseAngle - 10;
    Z_Yaw.FinalAngle = Z_Yaw.FuseAngle + 25;
    uart_print();
    vTaskDelay(5); // 调试可以用，实际应该关掉这个

    // Serial.println(String(Y_Pitch.FuseAngle) + "\r");
    // Serial.println(Z_Yaw.FuseAngle);
}